#include "CCapsule3d.h"
#include "CPlane.h"
#include "CTriangle3d.h"

boolean CCapsule3d::Sweep( const CVector3d& c0v0, const CVector3d& c0v1, fixed r1, 
						   const CVector3d& c1v0, const CVector3d& c1v1, fixed r2,
						   const CVector3d& v1, const CVector3d& v2, fixed timeElapsed, fixed& t )
{
	
// need to re-write so timeElapsed is not used
	CVector3d pt;
	
	CVector3d relVel( v1 - v2 );
	CVector3d relVelT( relVel );
	relVelT *= timeElapsed;
	fixed timebylengthsq = relVelT * relVelT;
	timebylengthsq = CMathFixed::Div( timeElapsed, timebylengthsq );
	CVector3d c0v2, c0v3;

	c0v2 = c0v0 + relVelT;
	c0v3 = c0v1 + relVelT;

	//boolean intersects = p.Intersects( c1v0, c1v1, pt, t );
	fixed u;

	CTriangle3d tri1( c0v0, c0v1, c0v2 );
	CTriangle3d tri2( c0v1, c0v2, c0v3 );

	if ( tri1.Intersects( c1v0, c1v1, u ) ||  tri2.Intersects( c1v0, c1v1, u ) )
	{
		// we need to compute t
		CVector3d pt = c1v0 + u * c1v1;
		pt = pt - c0v0;
		t = CMathFixed::Mul( pt * relVelT, timebylengthsq );
		return ( TRUE );
	}

	fixed tD = r1 + r2;
	tD = CMathFixed::Mul( tD, tD );

	CVector3d p,d,c1,c2;
	

	// we need to implement tri-seg closest point

	tri1.ClosestPoint( c1v0, c1v1, c1, c2 );
	d = c2 - c1;
	if ( d * d < tD )
	{
		d = c1 - c0v0;
		t = CMathFixed::Mul( d * relVelT, timebylengthsq );
		return TRUE;
	}

	tri2.ClosestPoint( c1v0, c1v1, c1, c2 );
	d = c2 - c1;
	if ( d * d < tD )
	{
		d = c1 - c0v0;
		t = CMathFixed::Mul( d * relVelT, timebylengthsq );
		return TRUE;
	}

	
	return FALSE;
}


boolean CCapsule3d::Sweep( const CVector3d& c0v0, const CVector3d& c0v1, fixed r1, 
					       const CVector3d& pos, fixed r2,
						   const CVector3d& v1, const CVector3d& v2, fixed& t )
{
	CVector3d c1, c2;
	CVector3d relVel ( v2 );
	relVel -= v1;

	fixed u0, u1;
	
	if ( CLineSegment3d::Intersects( c0v0, c0v1, pos, pos + relVel, &u0, &u1 ) )
	{
		if ( u1 < 0 )
		{
			// moving away, check for intersection at t = 0
			t = 0;
			return Intersects( c0v0, c0v1, r1, pos, r2 ); 
		}
		
		if ( u0 >= 0 && u0 < 1 )
		{
			

			// find distance at start, and distance at intersection is 0
			// this distance between a point and a stationary line varies linearly over time (because distance implies a rt triangle)
			
			CVector3d pt;
			CLineSegment3d::ClosestPoint( c0v0, c0v1, pos, pt );
			pt -= pos;
			fixed dist = pt.Length();
			
			t = CMathFixed::Div( CMathFixed::Mul( u1, dist - r1 - r2 ), dist );
			return TRUE;
		}
	}

	// using a real large value for u's since lines are almost parallel
	// also we find the end point most likely for collision

	CVector3d q( c0v0 );
	q -= pos;
	fixed d1 = q * q;
	q = c0v1 - pos;
	fixed d2 = q * q;

	if ( d1 < d2 )
		CLineSegment3d::ShortestVectorToSegment( c0v0, c0v1, pos, pos + CMathFixed_FloatToFixed( 10.0f ) * relVel, c1, c2, &u0, &u1 );
	else
		CLineSegment3d::ShortestVectorToSegment( c0v1, c0v0, pos, pos + CMathFixed_FloatToFixed( 10.0f ) * relVel, c1, c2, &u0, &u1 );

	c1 -= c2;
	r1 += r2;

	t = CMathFixed::Mul( u1, CMathFixed_FloatToFixed( 0.1f ) );

	if ( c1 * c1 < CMathFixed::Mul( r1, r1 ) && t > 0)
		return TRUE;

	return FALSE;
}


boolean CCapsule3d::Intersects( const CVector3d& c0v0, const CVector3d& c0v1, fixed r1, 
								const CPlane& plane )
{
	return FALSE;
}


boolean	CCapsule3d::Intersects( const CVector3d& c0v0, const CVector3d& c0v1, fixed r1, 
								const CVector3d& pos, fixed r2 )
{
	return FALSE;
}